COMPUTER AIDED SIMULATION OF 
THE DYNAMICS OF A MANIPULATOR 


by 

MADHU SHEKHAR, C. 








MEGHANICAL engineering 

INDIAN INSTITUTE OF TECHNOLOGY, EANPUB 

MAY. 1985 ^^^^ ^ ^ 



COMPUTER AIDED SIMULATION OF 
THE DYNAMICS OF A MANIPULATOR 


A Thesis Submitted 

In Partial Fulfilment of the Requirements 
for the Degree of 

MASTER OF TECHNOLOGY 


^ it ff 

by 

MADHU SHEKHAR, C. 


to the 


DEPARTMENT OF MECHANICAL ENGINEERING 

INDIAN INSTITUTE OP TECHNOLOGY, KANPUB 

MAY, 1985 



VI 


' Ilf K***^*" 

l‘»' ’ ■ . .iiiiSpl 


. ^ tifa. 


fY)£ - M & Sfi G ^ CCTT^ 



CERTIFICATE 


Certified that the present work entitled 
‘Computer Aided Simulation of the Dynamics of a 
Manipulator' has been carried out by Sri Madhu Shekhar# C» 
vadear - the supervision of Dr, B.Sahay# and has not been 
submitted elsewhere for a degree. 

Dr, B,Sahay has gone abroad to Canada for 
three months, and has authorised me to submit the 
certificate on his behalf and to look after the viva 
of Sri Madhu Shekhar, 



Assistant Professor 
Mechanical Engg, Department 
IIT, Kanpur 


May 1985 


ACKNOWLEDGEMENT 


I acknowledge with deep gratitude the 
invaluable help and constant motivation provided to me 
by my Thesis Advisor Dr. B. Sahay, without which this 
work would not have reached completion. 

I am also highly indebted to Dr. S.G.Dhande 
who readily consented to conduct my oral examination in 
the absence of Dr. B. Sahay. 

Various other people have contributed in 
various ways towards this project : The Computer Centre, 
Library, and Mechanical Department staff, and of course 
my friends. 



C.MADHU SHEKHAR 


1 . 


2 . 


3. 




CONTENTS 

Page 

List 

of symbols 

i 

Abstract 

ii 

INTRODUCTION 

1 

1,1 

Introduction 

1 

1.2 

Literature Review 

3 

1.3 

Outline of the present work 

6 

AODELLING 

10 

2.1 

Introduction 

10 

2.2 

Geometric Model 

13 

2.3 

Kinematic Model 

19 

2.4 

Dynamic Model 

24 

SIA4ULATI0N 

27 

3.1 

Introduction 

27 

3.2 

Simulation Algorithm 

30 

3.3 

Adjustment Blocks 

33 

3.4 

Other Dynamic Characteristics 

44 

THE 

PROGRAM MODULES 

47 

4.1 

Introduction 

47 

4.2 

Program Modules 

47 


4.2,1 Block Matrix Operator 

48 


4.2.2 Simulator 

55 


4.2.3 Command Interpreter 

60 


4.2.4 Editor 

66 



4.3 

Setting input data 

67 


4.4 

Running the program 

69 

5. 

RESULTS 

70 


5.1 

Results 

70 


5.2 

Conclusions 

70 


5.3 

Suggestions 

72 

6. 

REFRENCES 

73 

7. 

APPENDIX 

75 


A 1 

Transactions during the program run 

75 


LIST OF SYMBOLS 


Number of links of an Articulate Mechanical System 
(AMS) 

ith link of the AMS 

ith joint of the AMS 
Centre of gravity of L^ 

Body fixed coordinate frame of L^^ 

Origin of 
Link length 
Link twist 
Joint angle 
Joint distance 

Position vector of the centre of gravity of the link 

Mass of the link 

Inertia tensor of the link 

Transition block matrix 

Jacobian block matrix 

Internal coordinate vector 

External coordinate vector 

Drive vector 

Force block vector 

Moment block vector 


ii 


ABSTRACT 


Dynamic performance is one of the most significant 
factors in the design of manipulators particularly for fast 
and accurate robots recently developed. 

Until the end of the last decade, the choice of 
fhe manipulator , the actuator and the control system units 
was a subject of free speculation, frequently based on experi- 
ence but lacking any systematic method. Hence the need for 
developing certain criteria and procedures for a systematic 
design of the manipulator. In the design phase, there is 
no efficient means except simulation to investigate and 
evaluate the highly non Irnear and coupled systems. The 

of this work is to create a software tool for the simu- 
lation of all the dynamical values and characteristics of 
Manipulator operations in a particular task execution and 
fhiis permit a fast evaluation of a great number of different 
configurations. 
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CHAPTER I 
INTRODUCTION 

In 1920, the Czech writer Caret Kapek wrote 
a collective drama entitled R.U.R ( Rossum's Universal 
Robot ) and with its translation the word Robot entered the 
English vocabulary. 

For half a century the word Robot appeared on the 
pages of science fiction stories and novels, Issac Assimov 
coined the name of the trade, ’ Robotics ’, and provided 
us Roboticists with an ethic. These terms were later 
adopted by the scientific and technical community. 

The robot may be defined as a class of technical 
systems which imitate or substitute human locomotion or 
intellectual function. The Robot Institute of America 
gives a more precise and technical defination: ’ A robot is 
a reprogrammable multifunctional manipulator designed to 
move materials, parts, tools, or specialised devices, thro~ 
ugh variable programmed motions for the performance of a 
variety of tasks’. 

Although the size, structure, function and archit 
ecture of robots vary widely, all of them have certain 
essential features: 
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a) Manipulator 

b) Controller 

c) Actuators 

d) Sensors 

Robots posses several specific qualities in 
both a mechanical and a control sense. In the mechanical 
sense, a feature specific to manipulation robots is that 
they are open chain ‘active mechanism^ * , in contrast to 
conventional mechanisms in which motion is produced pri- 
marily' by the so called kinematic degrees of freedom. 

Also, they have a versatile structure, ranging from open to 
closed, from one to some other kind of boundary conditions. 
From a control viewpoint, robot systems represent, multi 
variable and essentially non-linear automatic control systems, 
A manipulation robot is also an example of dynamically coupled 
system, and the control task itself is a dynamic task. A 
further feature is the redundancy reflected in an excess of 
the degrees of freedom for producing certain functional move- 
ments of articulated mechanical system, IVhile it permits a 
greater mechanical flexibility in task performance, it comp- 
licates the control system by introducing optimising proce- 
dures for solving the problem of system redundancy. The 
surplus dof are used to satisfy special additional requests^f 
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A list of the uses of robots is impressive. Robots 
have been Used for operation in dangerous environments, in 
oceanography, space exploration, agriculture, medicines, etc. 

In industry, their scope is virtually boundless. The develo*- 
pment of automatically controlled industrial robots has wit- 
nessed three generations : Programmable, Supervisory Contro- 
lled, and Artificial Intelligence robots* The boom which we 
observe today in the sphere of industrial robots is not a 
mere tribute to vogue. These ’ steel collar workers' are 
now handling, transforming, assembling and dissembling parts, 
tools and specialised systems. In contrast to hard automation, 
robots represent ' soft ' automation - flexible, neither pro- 
duct, nor operations, nor industry limited, and immune to 
obsolescence. 

The distance separating Carel Kapek from Cybernetics 
was covered in a quarter of a century. The next twenty five 
year period built the foundations of what seemed pure fantasy. 
In this quarter we ar ' witnessing a quantitative accumulation 
and a qualitative leap in the field of Robotry* 

LITERATURE REVIEW 

Although Robotics is a multidisciplinary field, 
incorporating the resqits of numerous scientific and enginee- 
ring disciplines, two major areas can be identified: areas 
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involving mechanisms and areas involving control systems. 

In this work, we have confined ourselves only to the former: 
in particular to the dynamics of manipulators. 

The first work in the field of dynamics of spatial 
mechanisms was published by N.G, Bruyevich [®] as far back 
as in 1937. 

In 1963, H.J. Fletcher, L, Rongved and E.Y.Yu [W] 
studied the motion of a satellite composed of two rigid bodies, 
connected by a universal joint, under the load due to 
gravitation. 

In 1965, W.W. Hooker and G, Margulien [2 ] inspired 
by the proceeding work, studied the general case, where N+1 
bodies are connected by means of joints with one or two 
rotational degrees of freedom. Although this method was a 
significant advance in that it used matrix formalism, it was 
not able to obtain the matrices as functions of system state. 

R, E, Roberson and J. Wittenburg [Ml] in their 
approach have defined the system of bodies as a graph, whereby 
the elaborated, and known graph properties are used. 

The method of W.W, Hooker [JB] provides a new 
possibility of eliminating constraints but presents a seri- 
ous problem in deriving the motion equations. 
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The method of P,W. Likins [*] proposed in 1971# is 
a more direct application of the preceeding method. Moreover, 
without reducing the problem.’ s generality, it simplifies the 
numerical designation of the links in the chain, 

J ViJittenburg [4] generalised the method to systems 
having the structure of a topological branch whose joints 
permit r revolute dof's and t linear dof’s. It does not 
however, include fully closed chains, nor does it offer an 
explicit matrix procedure for obtaining the equations. 

In 1968, J.J. Uicker [3] proposed a method based 
on Lagrange’s equations for the study of dynamical behaviour 
of Joint connected systems of arbitrary structure. The method 
is sufficiently general to enable motion equations to be simu- 
lated on a computer. To overcome certain deficiencies in this 
method, M. Renaud [8i] proposed a method based on matrix cal- 
culus. In 1977, J. Zabala [5] proposed an algorithm based on 
M, Renaud’s method for automatically formulating motion 
equations. 

In 1974, E.P, Popov et al [■(] proposed an algorithm 
for solving the inverse problem based on the Gauss principle 
of numerical minimisation. The direct problem is solved from 
the necessary conditions of minimum. 
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In 1979, M, Vukobratovic and V, Potkonjak [M] 
proposed a method based on Appel's equations, and is 
computer oriented to a great extent. 

All the methods based on the general theorem of 
dynamics ( except the method of P*VJ. Likins ) are based on 
direct enumeration. However for an efficient computer 
solution, a recursive formulation is desirable. The methods 
using Newton's and Euler's methods are in principle complex 
due to the complexity of eliminating the constraints by forces 
and moments. Lagrangian equations provide the possibility of 
directly regarding the equations as functions of system cont- 
rol inputs. However, the inherent unsuitability of applying 
these equations lies in the need to calculate partial deriva- 
tives of Lagrangian functions, 

OUTLINE OF THE PRESENT WORK 

Modelling of the dynamics of Open Chain Active 
Mechanisms ( OGAM ) is a central point in the modern approach 
to manipulator design for two main reasons. One is connected 
with dynamic control, and the second is the development of 
procedures for optimal design of manipulators. 

As a rule, the methods for the dynamic analysis 
of active mechanisms use generalised coordinates. However, 
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in practice such a solution is insufficient, because one 
needs to consider the so called functional robot motion. 
This is a motion satisfying certain practical demands. It 
is therefore necessary to obtain drives producing these 
functional motions. Considerations of functional dynamics 
are closely connected with control and conversly. Hence, 
the term Dynamic Control, meaning, control based on detailed 
knowledge of the system dynamic characteristics has been 
introduced. 


OCAM have complicated behaviour including inter- 
action among multiple joints, non linear effects, and vary- 
ing inertia depending upon the arm configuration. In prac- 
tice until the end of the last decade, the designer proceeded 
with the design of each joint mechanism without knowing the 
actual characteristics. of the multi dof motion. Also, the 
choice of the kinematic scheme and its different parameters, 
the actuator units, and the control systems was a subject of 
free speculation, frequently based on experience but lacking 
any system of method. Hence many parameters, and often the 
motors were over powered. Hence the need for developing 
certain criteria and procedures for a systematic choice of 
manipulator configuration. In the design phase there is no 
efficient means except simulation to investigate and evalulate 
the highly non-linear and coupled systems. 
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The aim of this work is to develop an interactive 
program for the simulation of all the dynamical values and 
characteristics of the manipulator operations in a particular 
taks execution, permitting the designer to view the chara- 
cteristics in their entirety, and to evaluate a great number 
of different configurations as fast as possible. 

The program consists of four main parts. The 
first part defines all the vector, matrix and block matrix 
operations necessary for the simulation algorithm. 

The second part is the simulation algorithm. 

The algorithm automatically forms and solves the mathemati- 
cal model of the robot dynamics using the block matrix method. 
The inputs for the algorithm are the manipulator configura- 
tion, the initial state _nd the manipulation task. The 
output of the algorithm are the drives in the joints, the P-N 
diagram, the energy consumed for the performance of the task, 
etc. 

The third part is the Editor, It is used to 
perform interactive editing of the task, state or configu- 
ration of the manipulator, 

• The fourth part, the Command Interpreter, intre- 
prets the commands given by the user, demands the data to be 
provided, if necessary, then selects the desired primitives 
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and executes it. For the prevention of the user’s careless 
errors, the program traps them at various places and issues 
the relevent messages. 

The main program is V\/ritten in Pascal. The subro- 
utines for the drawing of graphs on the graphics terminal is 
written in Fortran. For Graphic action, PLOT-10 Interactive 
Graphics Library (IGL) is used. 
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CHAPTER II 
MODELLING 


2.1 INTRODUCTION; 

The algorithms for modelling Open Chain 
Active Mechanisms (OCAM) can generally be classified into 
two groups ; analytical and computer oriented. Analytical 
procedures have appeared before Computer , Aided (CA) methods 
and originate within the multibody satellite dynamics. 
Unfortunately the application of these methods ’ by hand ’ 
is very tedious and are subject to mistakes. Also these 
algorithms include either unsuitable numerical operations or 
impose some calculations which are not formalised. For these 
reasons it becomes necessary to develop CA methods for mathema 
tical modelling. It wilJ enable the designer to analyse a 
number of different configurations and choose the most appro- 
priate one to the future of the device. The development of 
CA methods which perform real time calculations of robot 
dynamics is a direct contribution to the synthesis of control 
algorithm for practical purposes* 

Any CA method must satisfy the following 

requirements; 

a) The input data for the algorithm are ; robot 

configuration, robot state, workspace state, and 
robot task. Using such input data the computer itself 
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forms and solves the mathematical model , it©*# 
robot dynamics. In principle three problems of 
dynamics may be solved : a direct and an inverse 
one, or a combination of these two, 

b) The algorithm includes ho numerical differentiation. 

The present CA methods may be divided into three 

groups : 

a) Methods based on the general theorem of dynamics 
and the Newton- Euler equations, 

b) Methods based on the second order Lagrange 
equations, 

c) Methods based on Gibbs-Appel equations and the 
Gauss principle. 

This program uses the general theorem of dynamics 
and incorporates the Block Matrix method. The Block Matrix 
method represents the analytically derived mathematical 
model but by using suitable block formalin the model redu- 
ces to the compact matrix form suitable for solving on a 
computer. 


The aim of each CA method is to derive functions 
f and g such that 

iJ = f(u,Cj ,P, mechanism configuration) 

P = g(u,u,d* mechanism configuration ) 
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where u is the generalised coordinate vector and P is the 
drive vector. 


The functions f and g are not some explicitly 
prescribed or derived function. They represent large com- 
I) putation algorighms. The realisation of the algorithms f 
and g are specific to and characteristic of each method and 
it depends on the mechanical approach. The algorithms f 
and g, although mutually inverse, (f represents the direct 
problem and g the inverse one ) are sometimes realised in 
rather different ways. Most of the CA methods consider the 
fifth class kinematic pairs only, i.e., joints permitting 
only one degree of freedom between the segments. If a com- 
pound joint is in question, then it is dissembled into a 
sequence of fifth class joints with small parameter segments 
between them. 

The following assumptions have been made while 
forming the mathematical model: 

a) All the links are rigid bodies. 

b) All the articulations are fifth class pairs. 

c) All the articulations are perfect. 

d) There are not kinematic singularities. 

The overall model is developed in three phases: 
Geometric, Kinematic, and Dynamic* 
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2.2 THE GEOMETxHIC MODEL: 

The kinematic structure of the arm has been 
modelled based on the Denavit and Hartenberg convention [ij. 

Referring to the Figure 2.1, the links of the 
Articulated Mechanical System (AMS) are numbered from zero 
at the base to N at the End Effector (EE). Links L^__-|_ and 
L. are connected at joint T. . The axis of a joint is def- 
ined as the axis of rotation for a revolute joint and as 
a line parallel to the generatrix for a prismatic joint, A 
convenient fixed coordinate system is chosen for the Aj/iS. 

A Body Fixed (BF) coordinate system is attached to 
each link L^ .The origin of frame is denoted by 0^ . 

The z axis of the frame is oriented along the axis 
of the joints Tj^ and thr x axis is directed along the 
common normal. The y axis is so chosen as to make a right 
handed coordinate system. The relative position of succe- 
ssive pair axes is described by the use of the unique 
common perpendicular between by axes of successive coor- 
dinate frames. With each link L^ is associated a vector 
defining the centre of mass of L^ with respect to the 
frame a mass and an inertia tenser J^. 

Four parameters are used to describe each 
successive joint and link pair: 
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Link length, a 


Link twist, a 


Joint angle, 0 


Joint distance, u - 


distance between successive 
z axes 

angle between successive 
z axes 

angle between successive 
X axes 

distance between successive 
X axes. 


Refering to Fig, 2.2, the transformation of the 
frame into the frame may be represented by the 

concatenation of the following elementary transformations: 


a) R (Zi.i, ©i) 


b) T 


c ) T ( X^ , 3 ) 


d) R (x^,a:^) 


rotation around z. , 

1-1 

by an angle 9 until 

becomes parallel to x^ 

translation along 

by a distance s until 

X. ^ coincides with x. 
1-1 1 


translation along Xj^^ 
by a distance a until 
^i-1 coincides with 

Rotation around by an 
angle a until and 


e. 

1 


coincide 
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From these four elementary transformations ^ a 
4X4 homogenous transformation matrix T. , which 

relates the position and orientation of frame to that 
of frame may easily be formed, 

r 1 

I Cos 0^ Sin 0^ 0 a^^-Cos 0^ 

-Sin 0. -Cos a. Cos 0.*Cos a. Sin a. a. - Sin G.t ‘«(2.l) 

I 1 jl Jl. IL 

T._1 .= !■ 

^ j Sin 0. 'Sin a. -Cos ©--Sin a. Cos a. „ 

II 111 

^ 0 0 0 1 _ 

Here, i orthogonal matrix as it trans- 

forms one orthogonal system into another. The transformation 
between other link coordinate systems may be obtained by the 
multiplication of intermediate transformation matrices. 

It should be noted that for a revolute joint, the 
joint angle is the joint variable ( the generalised 
coordinate ), while for a prismatic joint, it is the joint 
distance u^^. Similarly, the generalised force for a revolute 
joint is a torque, and for prismatic joint, it is a force, 

A point r^ in the frame Cp transforms to the point 
^q in the frame C according to the equation 

Hi 



( 2 . 2 ) 
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Let a 3 X 3 sub matrix of i giving 

the orientation of the axes of C._, v/ith reference to C. 

1 ^ i. 


\-l,i' 


Cos 0. 


Sin Oj, 


0 


■Sin 9.' Cos a. Cos G.- Cos a. Sin a. 
11111 

! Sin 0. • Sin a. -Cos©. -Sin a. Cos a. 
1 1 1 1 1 


,...(2.3) 


a 


And be 3 X 1 vector giving the position of 

the origin of the frame Cj__^ with reference to C^ 


1 . 


i-l,i = [ a^- Cos 0^ a^' Sin ] 


(2.4) 


The equation 2.2 may also be written as 


i-1 


= A. 


i--l,i ^i-l,i 


. . . . (2.5) 


Then for a vector V. ^ 


vP = A, 


V? 
p,q 1 


.... ( 2 , 6 ) 


where A^^^ is obtained by the multiplication of intermediate 
transformation matrices 


%fC[ \,P+1* ^p+l,p+2 ^q“l,q 

. is written simply as A. and 1 . as 1. 

u j 1 * .f O y 1 X 

Applying equations 2,5 and 2.6 repeatedly, we get 


i 

S 

j=0 


19 


r . 

1 


A. 




. . . . (2.7) 




A. 


1 



. . . . ( 2 . 8 ) 


The three columns of the matrix Aj^^j represents th© 
orientation of the axes of the frame in terms of the 
generalised coordinates, and the vector 1^^ represents the 
position vector of the centre of gravity of the EE in the 
external coordinate system. Thus the geometric model enables 
us to convert internal coordinates to external cordinates. 


2. .3 KINEMATIC MODEL: 


The derivation of the equations constituting the 
kinematic and dynamic model is very lengthy and complex, 
and is discussed in [6]. The analytical equations themselves 
are very long . However, by using block matrix formalism, 
the equations are reduced to a very compact form. The equa- 
tions, are presented here only in their final form [7], 


Let a^,o...^ be a set of vectors, and let us define 
the Block vectors a and a° of dimension ( NXl (3X1)) 


a 


[ 


a 


1 

1 » 





O r tT 

a - L ajL • • • • 

Let Eg be a 3 X 3 unit matrix, and a N X N 

unit matrix let V be the ( N X N ( 3X3 )) block matrix. 
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V = 


^ E. 


0 0 




n 


3 * * 


^ O ^ 


Further, let us define the link type matrix, 
S ( N X N ), and the block matrix ( N X N (3 X l))as 


follows 


a, ] 


S = diag [ 5^^ ^2 •*•••• 

= diag [ V 2 ^ 

T 

whGr© >;. = [0 Sin (X* Cos ^ 1 

and is an indicator, whose value is 0 if the ith 
joint is revolute and 1 if it is prismatic. 


. (2.9) 


Let A be the orientation block matrix ( N X N 


3 X 


3)) 


A 




^3 

A21 

^31 


0 

E, 

A. 


0 0 

0 ....... 0 


32 


% 


o i 


A 


A 


■N1 


■N2 




^ 3 ! 


Now, let us define two functions A and A as follows: 
0 -V. _ V. 


X(V.) = 


^iz 


-V. 

ly 


^V. 

iz 

0 


V. 

IX 


ly 

-^ix 


...(2.10) 


where 


[ V. V. V. ]■ 
IX xy iz-' 
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A(y) = diag [ \ (V. ) hiV^) 

where is a. vector ( 3X 1) 

T 

Let q = [ q 2 J vector of internal 

coordinates ( N X 1) and X X 2 •*.*. Xj^ ] f the v.ect03r 

of external coordinates ( N X 1) , If v is the linear velocity 
Vector^ and w , the angular velocity vector, then X =• [v w] 
is a ( 2 X 1^ (3 X 1)) velocity block matrix. 


Now, we have the following relations for velocity 

block matrix 

X ^ ,,.,( 2 . 12 ) 

where Bis a ( 2Xl( NXN ( 3 XI))) block matrix 


B = 


= - (A A (1) +A (P) A V (E~S) + Av 


B2 = 


A v( E-S ) 


.,,(2.13) 


For velocities expressed in the external coordinate system, 
we have 

B° q 


where 




> . 

o~ 


B 


B. 


.. (2.14) 


.. (2.15) 


bJ = -A* (p ) e° (E-S) ^ + VSe°^ 

V e° (E-S) A 
aiag ... e^j] 


®2 - 




0 

... 0 


• 



> 

11 


... 0 


* 

X(P ,J 

L oN 




Now, let w and. 6 be linear and angular acceleration 
vectors (3X1) respectively. Then ^ = [we] 

We have the following relation for acceleration 
block vector 

X = Bq + Dq ...(2.16) 

where 



D^= ( AA(1)+A(p) ) AA(w) V (E~S - (AA(oo)A(l) 

+ A(oj) A(p)) A3^( E-S ) + 2AA(w) vS 

□ 2 = A (A(w) a^(E-S) ...(2.17) 


In the external coordinate system, 


X° 


B°q 


+ D°q 


... (2.18) 


where 


0°= 


D? 

1 


D) 


2. 

D°= [dJ 


1 

^N J 


Dj= Q ( 




dimension ( N X N ( 3X1)) 
dimension ( 3 X N ) 
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d 


'li 


li 


0 




0 


0 


0 


0 


0 

0 

0 

.0 


...(2.19) 


^kj 


0 


= X( e.„p X(ej^) ( 1-Sj^) (1-Sj) 


*21 


d 


.2 

il 


0 


*12 

0 




*li 


,2 

^12' 


0 


*2i 


0 


0 


0 

0 

0 

0 


. . . 0 

. . . 0 


.0 


0 


2 

kj 


-1 ( =3 

2 

jk 



O CM 
Q 

= [D^ 

^ T* 

... ] dimension 


* 2 

= Q Hf 


Q 

= [ qi Eg 

♦ 

^2^3 


( N X N (3 X 1 )) 
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Rt = 


O 




a. 

0 




0 

0- 

0 


^ « * 


^ XC ® o ) ®i».i l**^i 2^^i 1* 


0 

o 

o 

o 


0 


Q 


*** 


0 


®i = ^-Si 


Vie now have the expressions for the velocity and 
acceleration of the links in the external coordinate system 
as well as the BF system. These expressions constitute the 
kinematic model. 


2.4 DYNM/lIC MODEL: 

The dynamic equations are derived on the basis of 
D ' Alambert*s principle by interrupting the chain ficti- 
tiously at a joint and balancing the forces and moments. In 
this way, N scalar equations are obtained, and transformed 
into matrix form using block formalism. 

Let us introduce the following block matrices of 
dimension ( NXN (3X3)). 
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m = diag [ 

J° = dlag [ J° j° ] 

= dlag [[ ] 


The expressions for the block vectors of the 
resultant inertial forces and moments can be written together 
as 


where 




o 


q 





m D° 


Q°J°B2 + J°D° 


Q = A (0)°) 


C°q 


.. .. ( 2 * 20 ) 


( 2 , 21 ) 


Let us further introduce the vector of the drives 
P of dimension ( NXl) 


3 - [ Pj. Pj^]^ 

Then the dynamic equations can be written, as 

Wg = + C» Mg + D* (G°+ f| ) + P 


« • r 


( 2 . 22 ) 



26 


where 

W = b°^P'b° 
b’= c® 

C = B2°^ 

D'= Bj^'^ ...(2.23) 


by introducing 


U (q,^) = B* + C« M° + D‘ (G°+F°) ....(2*24) 

the equation acquires the from 

W(q) = U (q,q) + P ...(2.25) 

Thus we have described the dynamics of the AMS by 
means of N differential equations in matrix form. 
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CHAPTER III 


SIMULATION 


3.1 INTRODUCTION; 

The notion of the simulation of dynamics 
usually involves the solution of the inverse problem of 
dynamics, i.e, the determination of the motion for prescribed 
generalised forces. In this case., the simulation is considered 
somewhat more liberally so that it also includes the notion 
of the simulation of the direct problem. 

The Fig, 3,1 shows the different models and 
the different levels of control of a typical manipulator orga- 
nised in a hierarchial fashion. The highest level defines 
the task to be carried out, the strategic level divides the 
imposed task into elementary functions, the tactical level 
performs the distribution of an elementary movement to the 
motion of each degree of freedom of the robot, and the exe- 
cutive level executes the imposed motion of each dof. The 
model developed in the previous chapter is used as the basis 
for the development of the algorithm for the simulation of the 
manipulator dynamics, and thus the synthesis of the tactical 
level. 


Before proceeding further, it will be useful 
to note an important point. The task of a manipulator may 
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be defined as the process of transferring the system state 
from one bounded region of initial states into another 
bounded region in the state space within a finite settling 
time, the system state being in the bounded region of the 
state space during the transfer. The state of the Articu- 
lated Mechanical System ( AMS) , at any instant may be descri 
bed by means of a vector of coordinates. The coordinates 
are called internal coordinates if the elements of the vector 
represent the position of the articulation at that time ins- 
tant, If they represent the coordinates of a predefined 
point on the End Effector (EE) with reference to the exter- 
nal coordinate system, then they are called the external 
coordinates . However, in practice, the task is prescribed 
neither by means of internal coordinates, nor the external 
coordinates, but by the s' called functional coordinates, 
as it is necessary to consider the functional robot motion. 
This is a motion satisfying certain practical demands. For 
instance, for a particular operation, the task of positioning 
the minimal configuration may be given in terms of spherical 
coordinates, and the task of orientation of the the EE may 
be given in terms of three Eular angles. For another oper- 
ation it may be more convenient to give the orientation of 
the EE in terms of a direction and an angle of rotation 


round it 
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Transformation of the functional coordinates 
into external coordinates is fairly simple, but the trans- 
formation of the external coordinates to internal coordin- 
ates is extremely complicated not only because the trans- 
formation is not explicit, but because it cannot even be 
numerically approximated due to the complexity of the systemo 
Also, a set of external coordinates may not have a unique 
set of corresponding internal coordinates. The external 
coordinates define the position and orientation of the EE , 
which we may call *pose' of the manipulator. On the other 
hand the internal coordinates determine the ’posture* of the 
manipulator, i.e,, the geometrical configuration adopted by 
the manipulator while holding an object in a particular pose. 
The posture of a manipulator completely defines it pose, but 
for a particular pose a m' lipulator may te in different 
postures, A 3 dof manipulator has only one posture in general, 
and a 6 dof one is believed to have 32 postures in general, 

A human arm can adopt an infinite number of postures. 

3.2 THE SIMULATION ALGORITHIVlr 

Let us designate by ri a function which transforms 
the internal coordinates into external coordinates. 


X = T) (q) 


(3.1) 
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Now, for the operation of the Computer Aided 
(CA) method of forming the mathematical model, it is nece- 
ssary to knov^/ q, q and q at each time instant, How'ever, 
only q appears as input since q and ^ are calculated by 
integration starting with known initial state q° and q°. 
So in order to realise the simulation, it is necessary to 
develop a procedure for calculating q from the knovm state 
q, 1 ^, and known external coordinate vector, X, 


Let 


By differentiating equation 3,1 twice, 


X = 


A 

dq* ^ 


X 


B 


^ a 
dq* ^ 

dq^ 


JX 


dq2 

d^p 

dq 


Then 


X = B tf + A 


...(3.2) 

... (3.3) 


... (3.4) 


B is called the Jacobian matrix (order NXN ). 

B and A are functions of the state q, and are calculated 
numerically by means of the kinematic model developed in 
the previous chapter. Then q is calculated by the relation 

-1 

= B ( X - A ) 




(3.5) 
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X can be represented by 

X = [ : ] 

where w is the linear acceleration, and e is the angular 
acceleration of the centre of gravity of the EE. 

From Eqn, 2.16 > we can obtain 

X = B?f + D(5 

= Bg +A ...(3.6) 

Now from Eqn. 2.25 we can calculate the drives 

P ~ 2 ( 2 , configuration) 

= Wq-U ... (3.7) 

By integrating q, we get the state q, ^ at the 
next time instant. With the new state and the corresponding 
value of the functional coordinates acceleration , Xp we can 
repeat the process. The simulation algorithm is presented in 
Fig. 3,2. 

3.3 ADJUSTMENT BLOCKS : 

As pointed out earlier, although the manipula- 
tion task can be defined fully by means of external coordina- 
tes, they are unsuitable for setting. For the various 
individual classes of tasks, the most suitable variables are 
given as inputs, which naturally reflect the type of the 


34 


task. These are called the, functional coordinates. The 
transformation of the functional coordinates to the exter- 
nal coordinates is done in a block, known as the adjustment 
block, within the simulation algorithm. 

Before proceeding with the various classes of 
tasks, let us define some notions precisely. By positioning 
we mean moving the centre of gravity of the last segment 
( or some predefined point on it ) to some desired point in 
the vrork space according to a prescribed motion law. 

Full orientation of the body in space means an 
exactly determined angular position of the body with respe- 
ct to the external space. 

Partial orientation of the body means that the 
given body axis coincides with a prescribed direction is 
space . 

To solve the positioning task, which is part of 
every manipulation task, three dof are necessary. 

To solve the positioning task along with the 
task of partial orientation, five dof are necessary. 

To solve the positioning task along with the 
task of full orientation, six dof are needed. 
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Now some typical classes of tasks and ways of 
using the dof will be analysed. 

a) a manipulator with four dof solves the positioning 
task by using three dof, and with the one remaining performs 
operations frequently sufficient for many practical tasks, 

1. The task is that of positioning in the cartesian 
coordinate system, i,e. x(t), y(t),z(t) for the 
centre of gravity of the EE, and the fourth dof 
is prescribed directly ( q^{t)) , 

2. Same as 1., but positioning is given in cylindri- 
cal coordinates (t), 9(t), z(t). 

3. Same as 1., but positioning is given in spherical 
coordinates r^t), 6(t), />(t). 

B) a manipulator with five dof solves the positioning and 
partial orientation task, 

1, The task is given in the form of positioning 
(A1 , A2 or A3) and partial orientation. 

C) A manipulator with six dof solves the positioning and 
full orientation task, as well as all the problems in which 
fev 7 er dof are needed ( when compensation of the dof surplus 
is done ). 
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1. The task is given in the form of positioning 
(A1,A2, or A3), and full orientation of the 
EE in terms of the three Euler angles, 9(t), 
ijj(t), /3(t). 

2. The task is given in the form of positioning 
(A1,A2, or A3 ) and full orientation is given 
in terms of one direction 0(t) and ^(t), and 
an angle of rotation around it, ijj (t). 

3. The task is given in terms of positioning as 
in 2 plus partial orientation. Motion along 
one dof is prescribed directly ( qj^(t)). 


The derivation of the adjustment blocks for 
these classes of task will now be given briefly, YIe will 
use the same of matrix notation for Xg (external coordinates )i .e - 

Xg = r;^] = Bq-fA 

Let B = [j^]] and A = [|j 


A1 The input to the block is Xp ( functional coordinates) 



Hence, w 


X y z q^ J 
L X y z ] 


... (3.7) 


Using the equation 3,5 qj^/q 2 calculated q^is 

prescribed directly. 
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.r r •* «• -iT 

^ Xp = [ P ©■ z ^4-' 

The following equations can easily be derived 
using elementary transformation. 
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... (3.8) 


, Wq 


• •• 

pO -i"2p6y ^^2 ^ 


where = ?? -* P© 

««• 

p , p* . 0, 6 etc are obtained by integration of p , © etc. 
the rest is as in Al. 

M , q4 J • 

Again, the following equations can easily be derived. 

Please refer to Fig. 3.3. a,b,c. 
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The positioning may also be prescribed in cylindrical or 
spherical coordinates. In any case, w is calculated as in 
Al,A2,or A3, as required. Please refer to Fig. 3.4. 

The angular acceleration is given by 

f ^ 

e = VTi Iflfl ® 


Vi/here 


Tt = 


*6 

«• 

0 

$■ 

+ 
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A 

i- 
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Cos ijJ 
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Sin ijj- Sin JB Cos ^ 0 

Sin ijj'Cos^-'— Sin^ 0 




~ 0 0 -Sin ijj 

-Sin ^ Sin ijj • Cosp Cos (jj^Sin l|j 

-Cos^s -Sinlil • Sinfi Cos l|J' Cos ^ 

A^ is the transition matrix of the last segment 
6 




... (3.10) 


^6 ~ ^ 


^6* 


C2 This method is suitable in many practical cases, for eg. 
spraying powder along a prescribed path, or screwing in a 

bolt. 

Xf = [ s Y z e f 


w 


is determined as in Cl 


41 


Refering to Fig. 3.5, the direction of any line may be 
prescribed by means of two angles 9 and f . Rotation aronn 

this line is given by ijj • 
e - is calculated from the relation 

e) = t A-^ - [ ••• 


where 


A = 


= 


^3 = 


^1 ^2 ^ 
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B1 In this case, the raanipulator has five dof , for which 
positioning is prescribed as in Al, A2 or A3 and the orien- 
tation is prescribed by the requirement that the EE should 
coincide with a given direction in space 

Xp = C *x y 2 ^ 

where the angles 9 and specify the direction, as in C2, 
w is calculated as in C2. 6 is claculated as follows. 



where 

h, = - Cos ^5^ Cos G -• Sin ^ p Cos 0 + 

•*- ^ 2 
2 Sin/> p Sin 0 9 — Cosp Cos© 0 

- Cosp Sin © 0 

ho = — Cospp ^ Sin 0 — 5±npp Sin 0 — 

.2 

- 2 Sin Cos © © - Cos ^ Sin © © 

+ Co s p Co s©@. •.••(3. 13 ) 

C3 In this case five functional coordinates are prescribed 
exactly as in B1 and the remaining dof is prescribed directly 
namely (t) 

Xp = [x -y 2 © ^ q^J 

The adjustment block is derived as in Bl, 
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3i4 OTHER DYNAI /iICAL CH/^.RACTERISTICS s 

(a) P- N Diagram; 

Since the drives and generalised velocities, 
q, are calculated at each step of the simulation, it is 
possible to draw a diagram for each joint, the characteristics 
of the driving motor torque P versus the motor rpm N, i.e. 
the diagram P-N for each motor* Such a diagram is very useful 
during the synthesis and choice of the servosystems . The 
producer gives the Pp^y “ ^ motor characteristics In the 
catalogue where P„_v maximal motor torque at motor 

rpm N. By comparing the necessary characteristics obtained by 
means of simulation with the one from the catalogue, one can 
decide whether the chosen motor suits its applications. 

The diagram P-N is obtained in such a way that 
for each time instant and for joint k , 

N = 

P = T] ( R ) Pj, / N 14 ) 

where R is the reduction ratio of the subject joint and r) (R) 
is its mechanical efficiency. Repeating the procedure for 

. : ‘i ' ■ , 

each time instant, the desired diagram is obtained. 
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(b) Energy Consumed s 

During the simulation, energy consumed during 
each step At^ is found, and the total energy is calculated 
by summation of these values, 
i A 

E = E ^ + E^^_ (3.15) 

where E^ is the total energy consumed including during the 
ith time step, and E is the energy consumed during the 
ith time interval. To calculate E the everage drive 
value on the interval is taken 

P^ed = I (3.16) 


Now, 





(3.17) 


where Aq^ = — q^”'^ . The elements of the vectors q^ and 

^med absolute value. The average drive value is used to 

avoid more complex interpolation. 


Vfe can thus obtain the total energy consumed for 
the given manipulation task. 

Thus, we now have a simulation algorithm which 
has the following inputs: the manipulator configuration, 
the manipulator state, and the manipulation task in the 
form most suited for prescribing ( according to the class of 
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functional movements). A- for the output, we obtain time 
history of the generalised coordinates, velocities of the 
drives, the torque-rpm diagram for each actuator, and the 
total energy consum.ed. 
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CHAPTER IV 
THE PROGRAM MODULES 


4.1 INTRODUCTIONS 

The simulation algorithm described in the 
previous chapter is implemented on a DEC 1090 mainframe in 
an interactive, user friendly environment. 

In one session, the user can set the input 
data, run the simulation routine and obtain the results 
on a VDU, a graphic terminal or the line printer, and save 
the output on the disk for future reference. If not satisfi- 
ed with the results, the user may change any of the input 
parameters on line using the Editor routine, and rerun the 
Simulator routine. To effect the above actions is the 
Command Interpreter routine, which accepts commands from the 
user, decodes it, and performs the necessary action. The 
commands are arranged in a hierarchial fashion with three 
levels. Errors, if any, resulting either from user action 
or during the program run are trapped, and relevant warnings 
or messages given to the user. 

4.2 THE PROGRAM MODULES; 

The program consist of four main modules. 
These will now be discussed in detail. 
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4*2,1 Block Matrix Opera' or ; 

This module consists of a number of procedures 
which define all the vector, matrix and block matrix operations 
necessary for the simulation algorithm. Let us use the foll- 
owing notation to describe the vectors > matrices, and block 
matrices ; 

VEC-3D A vector of dim.ension 1X3 

VEC-ND A vector of dimension IXN 

MAT— 3D A matrix of dimension 3X3 

MAT-ND A matrix of dimension NXN 

BLK— V— V A block vector of dimension IXN, whose 

elements are of type VEC-3D 

BLK-V-M A block vector (IXN) with elements of type 
MAT-3D 

ELK-M-V A block me ' rix (NXN) with elements of type 
VEC-3D 

BLK-M-M A block matrix (MXN) with elements of types 
MAT-3D 

BLK-V-V-V a block matrix (1X2) with elements of type 
BLK-V-V 

BLK-V-M-V A block matrix (1X2) with elements of type 
BLK -M-V 

BLK-M-M-M A block matrix (2X2) with elements of type 
BLK-fvIr-M. 
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An operation is named based on the type of 
the output variable. For instance, if a procedure takes 
two entities of type MAT— ND and after performing opera- 
tions returns an entity of type MAT-ND, it will be named 
as OPN-MND. Simularly, if a procedure returns an entity 
of type BLK-M~M, it will be named as OPN-M-M. However, 
for operations 'involving dot product of vectors, the 
keyword DPN will be used instead of OPN. For describing 
the operation, the follovi/ing notation will be used; 

< Operation > (< entity, entity. .> : <entity type>; 

< entity, entity. ,> ; ...); 

The following is the list of all the operations 
defined in this module. The last entity is always the 
output parameter. 

i) 0PN-V3D ( OPCODE: integer; S; real ; A; MAT-3D; 

B,C: VEC-3D; D: VEC-3D); 

Here and in all further operation?, OPCODE 
represents the operation code, whose value determines which 
operation is to be performed on the input parameters. In 
this case, the following operations are performed. 
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OPCODE OPERATION 

D = B+C 

2 D = B-C 

3 D = S.B 

4 D = A,B 

If the OPCODE is 1, then the vectors B and C are added 
according to the rules of the vector addition . Dummy 
variables may be passed for S and A. In this program, for 
dummy entities, a zero, or a null vector, or a null matrix, 
or a null block matrix is passed, depending upon the oper- 
ations . 

ii) OPN- VND ( OPCODE; integer; Srreal; A, B,C; VEC-ND); 
In Pascal, unlike Fortran, all the variables used must be 
declared, and the type checking is done at compile time. 

Hence, 0PN-V3D is separated from OPN-VND, although the two 
differ only in the dimensions of the parameters. The OPCODE 
and the operations are exactly same as for i). 

iii ) 0PN-M3D ( OPCODE: integer; S; real; A,B,C; 

MAT-3D); 

This is for doing operations on matrices. The Operations 
for the OPCODES are as follows 
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OPCODE OPERATION 

1 C = A + B 


2 

3 


C = A - B 


C = 


A 


T 


4 


C = S . A 


5 


C = A. . B 


This procedure is used, for instance, to calculate the 
transition matrix Aj^ in Eqn. 2.6 by repeatedly applying it. 
Further, it is used, along with OPN-V3D and OPN-VND, in almo 
st all the operation that follow. 


iv) OPN - MND ( OPCODE ; integer; S; real; A, B,Cj 

MAT-ND); 


This is same as OPN-M3D, except for the dimensions of the 
matrices A,B, and C, and the OPCODES have the same meaning. 
This procedure is called while evaluating the expressions 
in Eqn. 2.12 through 2.18. It is also called by some of 
the procedure that follow. 

v) OPNl-V-V (OPCODE: integer; S;VEC-ND; A:BLK-M-V; 

t 

B,C,D; BLK-V-V); 

The operations performed by this procedure for different 
values of OPCODE is as follows 
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OPCODE OPERATION 

1 D B.-+ C 

2 D = B - C 

3 D = S . A 

It is easy to see that while performing the operation 
D = B + C y the elements of B are added to the corresponding 
elements of 0 , These elements are not numbers, but vectors i 
Hence, to calculate D, this procedure invokes OPN-V3D, N 
times. Also, in this case, dummy variables are passed as 
S and A. 

vi ) 0PN2-V-V ( A ; BLK-V-M; B ; BLK~M~V ; C : BLK-V-V ) } 

No OPCODE is given here as only one operation is to be perfor' 
med, i.e., 

C = A . B 

An instance of the use of this operation is in Eqn. 2.19 for 
1 2 

calculating D^ and , 

vii) OPN-M-V (OPCODE; integer; S: MAT~ND; A; BLK-M-M; 

B,C,D;BLK-M-V); 

The OPCODE and corresponding operations are as follows; 

OPERATION 

D = B + C 

D = B - C 
T 

D = B . 

D = S . B 

D = A . B 


OPCODE 

1 

2 

3 

4 

5 
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This procedure calls 0PN-V3D repeatedly to evaluate D. Also, 
when OPCODE is 3, the transpose of B is evaluated, not the 
elements of B. This procedure is used in Eqn. 2.15 through 
2.19 to evaluate the expressions. 

viii) OPN-M-M ( OPCODE: integer; St MAT-ND; A, B,Ci 

BLK- M-M) ; 

This operates on block matrices whose elements are matrices, 
•and is used for evaluating the block matrices B, D and C in 
Eqn. 2.13 through 2.18 and 2.21. The OPCODES and the 
corresponding operations are as follows ; 


OPCODE 

1 

2 

3 

4 

5 


OPERATIONS 

C = A + B 

C = A - B 
T 

C = A . 

C = S . A 

C = A . B 


Again, it should be noted that to evaluate C, the elements 
of A and the corresponding ones in B are operated by OPN-M3D 
repeatedly as the elements are not numbers but matrices. 


ix ) OPNl-V-V-V (S:VEC-ND; A : BLK-V-M-V ; B : BLK-V-V-V ) ; 
As there is only one operation to be performed, OPCODE is not 

used. This does the operation 

B ^ A « S 
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To evaluate expressions like Bq and Dq in Eqn. 2.12 and 2.16, 
this procedure is used. 

x) 0PN2-V-V-V (A,B,C; BLK-V-V-V); 

This does the operation 

C = A + B 

Please note that as the elements of A and B are block matrices 
of type BLK-V-V, OPN-V-V is invoked repeatedly to evaluate 
C = A + B. This is used in Eqn. 2.16. 

xi) OPN-V-M-V (A; BLK-V-M-V; B; BLK-M-M-M; C:BLK“V“M-V) 
This is used in Eqn. 2. 23 and does the operation 

G = B . A 

xii) DPN-R (A, B; VEC-3D; R: real); 

This gives R as the dot product of two vectors A and B, and 
is used in the procedures that follow. 

xiii) DPN-V (A; BLK-M-V;B:BLK~V-V; C:VEC-ND); 

This performs the operation 

C = B . A 

To do this, DPN-R is called repeatedly as the elements of 
B and A are vectors. 

xiv) DPNl-M (A,B; BLK-M-V; C: MATr-ND) ; 

This does the operation 

C = A . B 
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XV ) DPN2--M (A,B: BLK-V-M-V; C-JAAT-m)} 

This does the operation 

C A . B 

by repeatedly applying DPNl-N to the elements of A and the 
corresponding ones of B and adding the matrices so obtained 
by OPN-MND. 

There is also a procedure for calculating the 
inverse of a matrix. 

xvi) INVERSE (DIM: integer; A,B: MAT-ND); 

B is returned as the inverse of A. DIM is the 
dimension of the matrices A and B. This procedure is used 
to calculate the inverse of the Jacobian matrix, in Eqn. 3.5 

4.2.2 Simulator : 

This module implements the simulation algorithm 
described in the previous chapter. This again consists of 
seven main submodules. 

The first submodule creates the link structures 
for storing the input data and initialises certain variables 
and files. The configurations parameters of the links of 
the manipulator are represented internally by means of a 
doubly linked circular list, and are accessed by means of 
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a pointer, as shown in Fig, 4,1 , One node of the link is 
designated as the Header, and contains a record consisting 
of all the configuration parameters like linklength, link 
twist, mass, inertia, centre of gravity, etc. Also, the 
record contains two pointers, a left pointer pointing to 
the last link, and a right pointer pointing to the second 
link. In the same way, the left pointer of the second link 
contains the address of the pointer to the first link, and 
the right pointer, of the third. V\Jhen the last link is 
reached its right pointer is linked to the first link. In 
the same manner, the generalised joint variable, q, velocity, 
q and the transition matrix A are stored in another link 
structure* The acceleration, in functional coordinates, 
describing the task of the manipulator, is stored as an 
array ( for a series of time instants ) of an array ( for 
each dof ) • 

Four input channels are initialised for 
reading from files TASK.DAT, CONFIG*DAT, CMND*DAT and 
PROG.HLP* The first two files obviously are for task and 
configuration data. The sets of commands used by the Comm- 
and Interpreter are stored in CMND.DAT, and PROG.HLP contains 
help for the user, which can be accessed while the program is 
running. The output files POSE.DAT, POSTR.DAT, DRIVE, DAT 
and PNDIAG.DAT, for storing the data for internal coordinates. 
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external coordinates, drives, and the P— N diagram respec- 
tively, are also initialised. Global variables like unit 
matrices, unit block matrices, matrices E and V in Eqn. 

2.13 and 2.15, null matrices, etc, are initialised. Certain 
option are also set to their default values. The system 
of units for both input and output data are set to MKS 
units, the representation for positioning and orientation 
are set to Cartesion and Euler ■ respectively ( Section 3.3 ) 

The second part of this module forms and 
solves the geometric and the kinematic . model. First, the 
functions X and A(Eqn. 2.10) are defined in procedures 
LAA©A and BIG-LAMDA. Another function, LAT^ASTAR is defined, 
which takes two indices i and j, and returns the matrix 
X (p j) (Eqn. 2.15). Next comes the procedure KINEMATIC. 

The geometric and kinematic modelling is done here. First 
the transition matrices are computed in the body fixed system, 
(Eqn. 2, 3) and then the matrices so obtained are concatenated 
to obtain the transition matrices in the external coordinate 
system. Next the block matrix is evaluated ( Eqn. 2, 15). 
Now, the block matrix’ D° is evaluated (Eqn. 2,19). Finally, 
internal velocities and accelerations are computed ( Eqn. 

2.14, 2,18). 
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The third submodule computes the Jacobian 
matrix and the vector A from the matrices B° and D° 
evaluated earlier, ( Eqn. 3.4 ), 

The fourth submodule is the adjustment block. 

The acceleration in functional coordinates are transformed 
into those in external coordinates, as discussed in Section 

3.3. 


Using the Jacobian matrix and the vector A obtained 
in submodule three, the accelerations in external coordinates 
obtained in submodule four are transformed into those in 
internal coordinates, in the submodule five. 

The sixth submodule is for dynamic modelling. 

Eqn, 2^20 through 2,25 are used and the drives, at a time 
t, obtained. Also, the torque and rpm are calculated for 
each joint at this time instant, as well as the energy consumed 
since the time instant ( t- At ), from the Eqn* 3.14 and 3.17. 

In the seventh submodule, the new state, q and q 
are computed by the integration of q over the time interval 
At , and the time is incremented by a^. 

The first two main modules have now been discussed. 
But the user will really not be concerned with these two 
modules. To use the program, what he is interested in is the 
commands to be given, and the Editor. 
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4.2.3 Command Interpreter (Cl)t 


This is for performing simulation, editing, 
input/ output, etc., as and when requested by the user. The 
commands are organised in a hierarchial fashion into three 
levels. The commands can be shortened until it becomes 
unique. If the command is misspelt, or an illegal comunand 
or an ambiguous command is given at any level, a message is 
given to the user, warning this. If a valid command is given 
then it is interpreted, and the control passed to the module 
required to perform the action desired. 

At the top level, a double arrow is given 
as a prompt . Now the user can type in any of the following 
commands; 


Command 
>> HELP 
>> TYPE INPUT 

>> LIST INPUT 

» SIMULATE 
>> TYPE OUTPUT 

>> LIST OUTPUT 


Action 

Give help for top level actions 
Display the input data on the 
terminal 

List the input data on the line 
pinter 

Perform simulation 

Display the output data on the 

terminal 

List the output data on the line 
pinter 
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>> PLOT Plot the graphs on the graphics 

terminal , 

>> EDIT Edit the input data. 

>> UPDATE Update the input file. 

>> DOCUMENT Type the Documentation. 

>> EXIT Exit from the top level. 

V^Jhen any of the input, output or edit action are requested 
by the user, the Cl in invoked recursively, and the user 
enters the second level. For input action, the second 
level prompt is a vertical line ’ [ ' * Now the user can 
type in any of the following commands: 

I help Type help for the input action 


j SYSTEM 

1 CONFIG 

1 STATE 

1 TASK- 
I EXIT 


commands. 

Type the system of units, coordinates, 
etc. 

Type the configuration parameters of 
the manipulator. 

Type the state parameters of the 
manipulator. 

Type the task of the manipulator . 

Exit this level. 


Fob output action, the second level prompt is a 
forward slash '/'• The user may now type in any of the 
following commands: 
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/ HELP 

/ DRIVE 
/ ORIENT 
/ POSITION 
/ VELOCITY 
/ ACCELER 
/ PM DIAG 
/ ENERGY 
/ EXIT 


i'ype help for output action 
commands . 

Display the drives in the joints. 
Display the joint orientation * 
Display the joint positions , 
Display the joint velocities. 
Display the joint accelerations. 
Display the P-M diagram. 

Display the energy consumed. 

Exit this level. 


For PLOT action, the second level prompt is a 
back slash *\ ’ . The commands are same as for output 
actions . 


For performing editing of the input data, when the 
EDIT command, is given, then the user enters the second 
level. The prompt is a percent sign V • Now the following 
commands can be given : 


HELP 

Type help for edit action. 

SYSTEM 

Change system of units,j'epresen 

tation of coordinates etc. 

CONFIG 

Edit configuration parameters. 

STATE 

Edit state parameters. 

TASK 

Edit manipulator task. 

EXIT 

End edit. 
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When one of the SYSTEM, CONFIG, STATE, or 
TASK command is given, the Cl is once again called recursive- 
ly, and the user thus inters the third level. The prompt 
is an asterix '**. If the user typed the SYSTEM command, the 
Editor asks for the system parameter to be changed. The user 
can now give the following answers: 


* System Parameter 

* System Parameter 
System Parameter 

* System Parameter 


UNITS 

ANGLE 

POSITION 

ORIENT 


The command interpreter is now once again invoked to 
determine the system of UNITS ( CGS,MKS or FPS), ANGLE 


(DEGREES or RADIANS), POSITION (CARTESIAN, CYLIFDRICAL, 
or SPHERICAL), ORIENT ( NONE, PARTIAL, FULL). If the system 
of orientation is partial or full , Cl is again called to 
determine whether the functional coordinates are Euler 
angles, a vector and an angle, etc ( as discussed in 
section 3.3 ). For instance, 

* System of representation for Orientation: EULER. 

In response to the CONFIG command, the eoitor gives 

the following prompt, 

* Add node, delete node, or change node? 
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If the state parameters are to be edited, then the 
second level Editor command STATE can be given. 

STATE 

Now the C.I. is invoked ( third level ) to determine the 
state parameter to be changed, 

* State Parameter: 

Now the following commands may be given : 

* State parameter : INITIAL VELOCITY 

* State parameter : INITIAL POSIT 

Now the vectors may be typed in as directed by the editor. 
Similarly, by issuing the second level /' TASK command, the 
task parameters may be changed. The editor takes a number 
K from the user and changes the acceleration vector at the 
Kth time instant. • 

The user may make as many changes as the desires, 
then exit the editor module. Now the top level command 
SIMULATE may be given to rerun the simulation module. Now, 
the output parameters may be observed and evaluated. If the 
user is satisfied with the results, then he may issue the 
UPDATE command, which will update the input files CNF'G.DAT 
and TASK. DAT. This cycle the user may continue as many times 


as he desires. 


66 


All the valid commands to the Cl are stored in the 
file GMNDiDAT as sets of commands* ViJhen the Cl is called, 
a parameter is passed to it indicating which set of commands 
it should accept from the user. In a command set, for each 
command coreesponds an action to be performed. The action 
for a particular command may include a call to the Cl. By 
making such recursive calls to the Cl, a hierarchial command 
structure is built. Control can pass to the higher level 
only after the lower level releases the Cl. Such a structure 
allows new commands to be easily incorporated at any level, 
to cater to new modules which may be added to the program. 

4,2.4 Editor Module; 

The editor may be called from the top level to 
interactively edit any of the input parameters. After 
editing the new input parameters may be saved by updating 
the file. All the Edit mode coimifiands are described in 

Section 4,2.3, 

Apart from the above four main modules the program 

also contains two other small modules: The HELP module for 

giving help to the user at various levels, and the UTILITY 
module Which contains utility routines for performing certain 
actions which, though not directly related to the actual 
running of the program, help to make it more useable. 
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There are standard procedures for reading integers 
and reals from a device. Suppose the program is waiting 
for the user to give an real number from the terminal i If by 
mistake the user types an integer^ or some character, the 
program is interupted, and control returned to the monitor. 

To avoid this, two procedures, READIN, and READRL are written, 
which dont read the numbers directly^ bat as a sequence of 
characters which are converted into their ASCII code and 
then the number computed. 


The whole program, except for the plotting routine is 
written in Pascal. The plotting routine, which calls certain 
routines from the Interactive, Graphics Library, PLOT-10, 
is written in Fortran, as these routines are only Fortran- 


Callable. The object code for the plotting routine and the 
rest of the program is li^.Ked together using the Fortran 
I/O Link Facility, FTNLNK, available here on DEC 10. 


4.3 SETTING INPUT DATA; ' 

,The data regarding the configuration, state, or 
task of the manipulator are initiallY given through disk 
files. These parameters may later be edited from the 

terminal. 


is given 


The data for the configuration of the manipulator 
in the file CNFG . DAT in the following format: 
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NO OF LINKS 
GRAVITY FORCE VECTOR 

LII'JKTYPE/LINK LENGTH/LINK TVIIST /JOINT ANGLE or 

JOINT DISTANCE 

COG VECTOR 
MASS 

INERTIA TENSOR 
EXTERNAL FORCE VECTOR 
EXTERNAL MOMENT VECTOR 
REDUCTION ratio/ JOINT EFFICIENCY 
A blank line or comment 

LINKTYPE/LINKLENTH /LINK TWIST/ JOINT ANGLE or 

JOINT DISTANCE 


COG VECTOR 


( for N links) 

The LINKTYPE is given as 'R’ for a revolute joint, and ’P’ 

for a prismatic joint. The COG vector is given as ( P 2 ^3^ 

and the inertia tensor J as ( J^^ '^12 "^13 “^22 23 31 
In this way, the data for N links are given. 

The data for the task of the manipulator, and the 
initial state are given in the file tASK.DAT in the, following 


formats : 
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INITIAL POSITION VECTOR 
INITIAL VELOCITY VECTOR 
A blank line or comment 
FUNCTIONAL COORDINATES ACCELERATION 
FUNCTIONAL COORDINATES ACCELERATION 


FUNCTIONAL COORDINATES ACCELERATION 
All these vectors are of the dimensions ( IXN ) * The units 
for these values can be given in either MKS or CGS or FPS 
units. By default, MKS system of units is assumed. 

4.4 RUNNING THE PROGRAM; 

To start the program, the following monitor command 

is given 

.RUN ROBOTA 

Now the control is passed to the top level Cl and the flow 
of execution will now totally depend upon the commands 

given by the user. 
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CHAPTER V:. 

RESULTS, CONCLUSIONS AND SUGGESTIONS 

5.1 RESULTS ; 

The program is run on a DEC — 10 computer using 
a sample input data. The input and output data, together 
with the transactions in a single session are listed in 
Appendix A 1. The listing illustrates some of the facili- 
ties provided by the Editor. Also appendend with it are 
the drive time, state time, and torque-rpm graphs. The output 
data obtained agrees with the actual values computed manually. 
The program listing is given in Appendix A 2. 

5.2 CONCLUSIONS; 

As pointed o'.it earlier, an algorithm which 
calculates the dynamic characteristics of a manipulator 
during the execution of a task clearly represents a useful 
means for the process of manipulater design. The algorithm 
chosen for this work forms and solves the dynamic model 
of the manipulator, and using incremental iterative displa- 
cement technique, computes the dynamic characteristics at 
a series of time instants. The dynamic model is based on 
the general theorem of Dynamics, and uses block matrix 
formalism to reduce the complex analytical equations into 
a compact form suitable for solving on a computer. The 
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inputs to the algorithm are the manipulator configuration 
( described by a set of parameters defining the geometric 
and inertial properties of the link), the initial state, and 
the manipulation task ( in the form most suited for prescri- 
bing, according to the class of the functional movements). 

As the output, WG obtain the time history of the generalised 
coordinates, velocities and drives, the P-N diagram, and the 
total energy consumed. 

This algorithm is implemented in the increasin- 
gly popular Pascal language, to aid the designer in a fast 
analysis of the dynamic characteristics of a number of 
manipulator configuration. The P-M diagram can be used to 
give specifications for the actuator units. The total 
energy consumed for a particular task can be compared for 
different configurations and the configuration chosen 
accordingly. The various graphs can be viewed on a graphics 

terminal. 

Any of the input parameters can be changed, 
by means of the Editor module and simulation done again. 
Further, the data may be given in any units. The program 
is command driven, i.e. the user types in commands for the 
action he desires. The command is decoded, and the action 
taken, by the Command Interpreter module . The Editor and 
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the Command Interpreter thus allow the user to analyse 
different configurations in an interactive manner* Further 
the structured development of the program allows new modules 
to be easily added. 

S'. 3 SUGGESTIONS: 

Improvements in the program can be made in 
two ways. One is an extension of the Dynamic module to 
include a manipulator with third and fourth class pairs, 
and a branched structure, and a manipulator with elastic 
segments . Secondly, other modules can be added to consi- 
der other problems in Robotics, like path synthesis, 
workspace synthesis actuator modelling , performance 
evaluation, etc. If this is done, coupled with the extensive 
use of computer graphics, then the gap between the designers 
and theoreticians can be bridged. 
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